﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

using MathNet.Numerics;
using MathNet.Numerics.LinearAlgebra;
using OpenNI;


namespace BTKinect.DataType
{
    class KinectMatrix : Matrix
    {
        #region CONSTRUCTORS
        public KinectMatrix() : base(3, 3) { }
        public KinectMatrix(double[,] A) : base(A) { }
        public KinectMatrix(double[][] A) : base(A) { }
        public KinectMatrix(double[] vals, int m) : base(vals, m) { }
        public KinectMatrix(int m, double s) : base(m, s) { }
        public KinectMatrix(int m, int n) : base(m, n) { }
        public KinectMatrix(int m, int n, double s) : base(m, n, s) { }
        public KinectMatrix(KinectPoint3D pnt1, KinectPoint3D pnt2, KinectPoint3D pnt3)
            : base(3, 3)
        {
            //use explicite conversion from KinectPoint3D to KinectVector
            this.SetColumnVector((Vector)pnt1, 0); //col1
            this.SetColumnVector((Vector)pnt2, 1); //col2
            this.SetColumnVector((Vector)pnt3, 2); //col3
        }
        public KinectMatrix(KinectVector columnVector1, KinectVector columnVector2, KinectVector columnVector3)
            : base(3, 3)
        {
            //use explicite conversion from KinectPoint3D to KinectVector
            this.SetColumnVector(columnVector1, 0); //col1
            this.SetColumnVector(columnVector2, 1); //col2
            this.SetColumnVector(columnVector3, 2); //col3
        }
        public KinectMatrix(Matrix m)
            : base(3, 3)
        {
            KinectMatrix k = this;
            k.SetColumnVector(m.GetColumnVector(0), 0);
            k.SetColumnVector(m.GetColumnVector(1), 1);
            k.SetColumnVector(m.GetColumnVector(2), 2);
        }
        #endregion

        #region OPERATORS/CONVERSIONS
        /// <summary>
        /// Subtracts a element of a vector from each element of a matrix row. The length of the vector
        /// and the number of rows of the matrix must be equal.
        /// </summary>
        /// <param name="matrix"></param>
        /// <param name="vector"></param>
        /// <returns></returns>
        public static KinectMatrix operator -(KinectMatrix matrix, KinectVector vector)
        {
            //define columns
            Vector v0 = matrix.GetRowVector(0) - vector[0];
            Vector v1 = matrix.GetRowVector(1) - vector[1];
            Vector v2 = matrix.GetRowVector(2) - vector[2];

            //calculate and save columnvectors in Matrix 
            KinectMatrix km = new KinectMatrix();
            km.SetRowVector(v0, 0);
            km.SetRowVector(v1, 1);
            km.SetRowVector(v2, 2);
            return new KinectMatrix(km);
        }
        /// <summary>
        /// Add element x of vector to each element of the x-th matrix row
        /// </summary>
        /// <param name="matrix"></param>
        /// <param name="vector"></param>
        /// <returns></returns>
        public static KinectMatrix operator +(KinectMatrix matrix, KinectVector vector)
        {
            //define columns
            Vector v0 = matrix.GetRowVector(0) + vector[0];
            Vector v1 = matrix.GetRowVector(1) + vector[1];
            Vector v2 = matrix.GetRowVector(2) + vector[2];

            //calculate and save columnvectors in Matrix 
            KinectMatrix km = new KinectMatrix();
            km.SetRowVector(v0, 0);
            km.SetRowVector(v1, 1);
            km.SetRowVector(v2, 2);
            return new KinectMatrix(km);
        }
 
        /// <summary>
        /// subtract column# of matrix1 from column#
        /// </summary>
        /// <param name="matrix1"></param>
        /// <param name="matrix2"></param>
        /// <returns></returns>
        public static KinectMatrix operator -(KinectMatrix matrix1, KinectMatrix matrix2)
        {
            //define columns
            Vector m1_0 = matrix1.GetRowVector(0);
            Vector m1_1 = matrix1.GetRowVector(1);
            Vector m1_2 = matrix1.GetRowVector(2);
            Vector m2_0 = matrix2.GetRowVector(0);
            Vector m2_1 = matrix2.GetRowVector(1);
            Vector m2_2 = matrix2.GetRowVector(2);

            //calculate and save columnvectors in Matrix 
            double[][] D = new double[][]{  m1_0 - m2_0,
                                            m1_1 - m2_1,
                                            m1_2 - m2_2
                                         };
            return new KinectMatrix(D);
        }
        public static KinectMatrix operator +(KinectMatrix matrix1, KinectMatrix matrix2)
        {
            //define columns
            Vector m1_0 = matrix1.GetRowVector(0);
            Vector m1_1 = matrix1.GetRowVector(1);
            Vector m1_2 = matrix1.GetRowVector(2);
            Vector m2_0 = matrix2.GetRowVector(0);
            Vector m2_1 = matrix2.GetRowVector(1);
            Vector m2_2 = matrix2.GetRowVector(2);

            //calculate and save columnvectors in Matrix 
            double[][] D = new double[][]{  m1_0 + m2_0,
                                            m1_1 + m2_1,
                                            m1_2 + m2_2
                                         };
            return new KinectMatrix(D);
        }
        public static explicit operator KinectPoint3D[](KinectMatrix m)
        {
            KinectPoint3D[] pnt = new KinectPoint3D[3];
            if (m.RowCount != 3 || m.ColumnCount != 3)
                throw new System.ArgumentException("need a 3D matrix!", "original");
            else
            {
                KinectVector kv1 = new KinectVector(m.GetColumnVector(0));
                KinectVector kv2 = new KinectVector(m.GetColumnVector(1));
                KinectVector kv3 = new KinectVector(m.GetColumnVector(2));

                pnt[0] = (KinectPoint3D)kv1;
                pnt[1] = (KinectPoint3D)kv2;
                pnt[2] = (KinectPoint3D)kv3;
            }
            return pnt;
        }
        #endregion

        #region  METHODS
        /// <summary>
        /// Round the values of a matrix. This is mostly used to prevent that a pseudo accuracy
        /// is presented.
        /// </summary>
        /// <param name="m"></param>
        /// <param name="decimals"></param>
        /// <returns></returns>     
        public KinectMatrix RoundMatrix(int decimals)
        {
            for (int i = 0; i < this.RowCount; i++)
            {
                for (int j = 0; j < this.ColumnCount; j++)
                {
                    this[i, j] = Math.Round(this[i, j], decimals);
                }
            }
            return this;
        }

        public KinectVector Distance(KinectMatrix km)
        {
            //get 3DPoints
            KinectPoint3D pnt1Matrix1 = this.GetRowVector(0);
            KinectPoint3D pnt2Matrix1 = this.GetRowVector(1);
            KinectPoint3D pnt3Matrix1 = this.GetRowVector(2);

            KinectPoint3D pnt1Matrix2 = km.GetRowVector(0);
            KinectPoint3D pnt2Matrix2 = km.GetRowVector(1);
            KinectPoint3D pnt3Matrix2 = km.GetRowVector(2);

            //calculate distances
            KinectVector tmpVector = new KinectVector(3);
            tmpVector[0] = pnt1Matrix1.Distance(pnt1Matrix2);
            tmpVector[1] = pnt2Matrix1.Distance(pnt2Matrix2);
            tmpVector[2] = pnt3Matrix1.Distance(pnt3Matrix2);

            return tmpVector;
        }
        #endregion
    }
}
